using System;
using UnityEngine;
using DanielSig;

namespace DanielSig.UnityExtensions
{
	public static class XRigidbody
	{
		public static Vector3 GetLocalVelocity(this Rigidbody body)
		{
			return body.transform.ToLocalDirection(body.velocity);
		}
		public static Vector3 GetLocalAngularVelocity(this Rigidbody body)
		{
			Vector3 axis;
			float angle;
			Quaternion.Euler(body.angularVelocity).ToAngleAxis(out angle, out axis);
			axis = body.transform.ToLocalDirection(axis);
			return Quaternion.AngleAxis(angle, axis).eulerAngles.GetClampedAngles();
		}
		public static void PullTowards(this Rigidbody body, Vector3 worldPosition, float force, float dampRadius, ForceMode mode)
		{
			Vector3 v = worldPosition - body.position;
			float dampMultiplier = (v.magnitude / dampRadius.Max(0.01F)).Min(1F);
			body.AddForce(v.normalized * force * dampMultiplier, mode);
		}
		public static void PullTowardsRotation(this Rigidbody body, Quaternion worldRotation, float force, float dampDegrees, ForceMode mode)
		{
			Vector3 forward = (worldRotation * Vector3.forward) - (body.rotation * Vector3.forward);
			Vector3 up = (worldRotation * Vector3.up) - (body.rotation * Vector3.up);
			//Vector3 right = (worldRotation * Vector3.right) - (body.rotation * Vector3.right);
			
			float dampMultiplier = ((worldRotation.eulerAngles - body.rotation.eulerAngles).GetClampedAngles().magnitude / dampDegrees.Max(0.01F)).Min(1F);
			
			body.AddForceAtPosition(forward * force * dampMultiplier, body.worldCenterOfMass + body.rotation * Vector3.forward, mode);
			body.AddForceAtPosition(up * force * dampMultiplier, body.worldCenterOfMass + body.rotation * Vector3.up, mode);
			
			Debug.DrawRay(body.worldCenterOfMass + body.rotation * Vector3.forward, forward, Color.red.SetAlpha(0.1F), 0.5F);
			Debug.DrawRay(body.worldCenterOfMass + body.rotation * Vector3.forward + forward * 0.8F, forward * 0.2F, Color.red, 0.5F);
			Debug.DrawRay(body.worldCenterOfMass + body.rotation * Vector3.up, up, Color.blue.SetAlpha(0.1F), 0.5F);
			Debug.DrawRay(body.worldCenterOfMass + body.rotation * Vector3.up + up * 0.8F, up * 0.2F, Color.blue, 0.5F);
			
			Debug.DrawRay(body.worldCenterOfMass + Vector3.down, Vector3.up * 2, Color.green);
			Debug.DrawRay(body.worldCenterOfMass + Vector3.left, Vector3.right * 2, Color.green);
			Debug.DrawRay(body.worldCenterOfMass + Vector3.back, Vector3.forward * 2, Color.green);
			//body.AddForceAtPosition(right, body.worldCenterOfMass + body.rotation * Vector3.right * force * dampMultiplier, mode);
		}
	}
}

